import java.io.FileInputStream;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.ObjectInputStream;
import java.io.ObjectOutputStream;
import java.util.Hashtable;
import java.util.Vector;
import java.awt.geom.Point2D.Double;

public class Environment {
	private Track track; 	//The track
	private Motion motion;	//Motion class is used to calculate movement.
	private Hashtable<Car, Agent> drivers = new Hashtable<Car, Agent>(); //For matching the correct agent to the correct car
	private int line_of_sight = 3; //This defines how far the driver can see in each direction. Total viewsize is (2*lineofsight+1)^2
	private Vector<Car> cars = new Vector<Car>();
	private Vector<Car> finishedCars = new Vector<Car>();
	private boolean visible; 	//This is used to check if we are using visuals or not.
	private CircuitDraw draw; 	//Used to draw the track if we're using visuals.
	private double acreward = 0;	//Used to calculate accumulative reward.

	
	

	//Constructor. Parameters speak for themselves.
	Environment(String trackname, int numberofcars, boolean visibility, int carmaxspeed,double learnspeed, int hiddenNeurons) throws IOException{
		track = new Track(trackname);
		Car newcar;
		Agent newagent;
		motion = new Motion(track);
		int[][] startplaces = track.getStart();
		if(numberofcars>startplaces.length) {
			System.err.println("Error: More cars than there are starting places!");
			return;
		}
		
		//Adds all the cars and their accompanying agents to the different startpositions (with speeds 0 and direction 0)
		for(int i = 0; i<numberofcars;i++){
			newcar = new Car(startplaces[i][0],startplaces[i][1],carmaxspeed);
			newcar.name = (char) (i+65);
			newagent = new ReinforceSarsaSpeed(line_of_sight,line_of_sight,learnspeed, hiddenNeurons);
			cars.add(newcar);
			drivers.put(newcar, newagent);
		}
		
		//Checks whether we want to see it
		if(visibility) {
			visible = true;
			draw = new CircuitDraw(track);
			draw.setVisible(true);
			draw.update(cars);
		
		}
		else visible = false;
		
	}
	
	
	//Sets a random agent instead of the normal one.
	public void setRandomAgent() {
		for(int i = 0; i<cars.size();i++) drivers.put(cars.get(i), new RandomAgent(line_of_sight, line_of_sight));
		
	}
	
	//Changes the exploration rate for each car
	public void setExploration(double exploration)
	{
		for (Car car : cars)
		{
			drivers.get(car).setExplorationChance(exploration);
		}
	}
	
	public void setDiscount(double discount)
	{
		for (Car car : cars)
		{
			drivers.get(car).setDiscount(discount);
		}
	}
	
	public void setLambda(double lambda)
	{
		for (Car car : cars)
		{
			drivers.get(car).setLambda(lambda);
		}
	}
	
	
	//Updates the car coordinates in the track.
	//This has to be done so other cars and the motion class can check whether a car is in the way or not.
	public void updateCarcoords() {
		int[][] output = new int[cars.size()][2];
		for(int i = 0;i<cars.size();i++) {
			output[i][0] = cars.get(i).getX();
			output[i][1] = cars.get(i).getY();
		}
		track.setCarpositions(output);
	}
	

	//Sets the visuals On or Off.
	public void setVisible(boolean yesnoorwhat) throws IOException {
		if(yesnoorwhat) {
			visible = true;
			draw = new CircuitDraw(track);
			draw.setVisible(true);
			draw.update(cars);
		}
		else visible=false;
		
	}
	

	//Saves the agents to a filename.
	public void saveNN(String filename){
		Agent[] agents = new Agent[cars.size()];
		for(int i = 0; i<agents.length;i++) agents[i] = drivers.get(cars.get(i));
				
		FileOutputStream fos = null;
		ObjectOutputStream out = null;
		
		
		try
		{
			fos = new FileOutputStream(filename);
			out = new ObjectOutputStream(fos);
			out.writeObject(agents);
			out.close();
		}
		catch(IOException ex)
		{
			ex.printStackTrace();
		}
		
	}
	

	//Restores agents from a filename
	public void restoreNN(String filename) throws ClassNotFoundException{
		Agent[] agents = null;
		FileInputStream fis = null;
		ObjectInputStream in = null;
		try
		 {
		     fis = new FileInputStream(filename);
		     in = new ObjectInputStream(fis);
		     agents = (Agent[]) in.readObject();
		     in.close();
		   }
		 catch(IOException ex)
			{
				ex.printStackTrace();
			}
		 for(int i = 0;i<cars.size();i++) {
		      agents[i].setLearnspeed(0.0);
		      drivers.put(cars.get(i), agents[i]);
		  }
		 
	}
	
		
	//Resets the track. 
	//Every car is back in the startposition with initial values.
	public void reset() {
		Car tempcar;
		int[][] startpoints = track.getStart();
		for(int i=0; i<finishedCars.size(); i++) {
			tempcar = finishedCars.get(i);
			tempcar.setCheckpoint(0);
			tempcar.setX(startpoints[i][0]);
			tempcar.setY(startpoints[i][1]);
			tempcar.setVX(0);
			tempcar.setVY(0);
			tempcar.celerate(0);
			tempcar.steer(0);
			cars.add(tempcar);
		}
		finishedCars.clear();	
	}
	
	//Gets a subarray which depicts the relative view a car has at that moment.
	public double[][] getRelView(Car car) {
		car.updateVxVy();
		double vX = car.getVX();
		double vY = car.getVY();
		int rotation = car.getDirection();
		int size = 2*line_of_sight + 1;
		int x = car.getX();
		int y = car.getY();
		double dx = Math.cos(Math.toRadians(45 + rotation)) * Math.sqrt(2) * line_of_sight * -1;
		double dy = Math.cos(Math.toRadians(45 - rotation)) * Math.sqrt(2) * line_of_sight * -1;
		int cornerX = (int) Math.round(x + vX + dx);
		int cornerY = (int) Math.round(y + vY + dy);
		//System.out.println(cornerX + " + " + cornerY);
		return track.turnLine(cornerX, cornerY, rotation, size);
	}
	
		
	
	
	//Old function for getting a view. This view wasn't rotated according to the car's rotation.
	public double[][] getView(Car kar) {
		int x1 = kar.getX()-line_of_sight;
		int x2 = kar.getX()+line_of_sight;
		int y1 = kar.getY()-line_of_sight;
		int y2 = kar.getY()+line_of_sight;
		return track.getSubArray(x1, y1, x2, y2);
	}
	
	//This is just used for testing purposes, it does not use the motion class
	public void step(int vvx, int vvy) {
		for(int i = 0; i<cars.size();i++) {
			Car car = cars.get(i);
							
			car.updateSpeeds(vvx, vvy);
			car.setX((int) (car.getX()+car.getVX()));
			car.setY((int) (car.getY()+car.getVY()));
			cars.set(i,car);
			
			if(visible) draw.update(cars);
			
		}
	}
	
	//The big step method which calculates each step the agent takes. Overall it does the following:
	//-Gets an action from the agent with the input values
	//-Updates the position of the car with the action and it's previous values.
	//-Checks if it passed a checkpoint and if so, was it the last one?
	//-Rewards the agent accordingly
	public boolean step(){
		for(int i = 0; i<cars.size();i++) {
			updateCarcoords();
			Car car = cars.get(i);
			Agent agent = drivers.get(car);
			
			int checkpoint = car.getCheckpoint();
			double[][] view = getRelView(car);
					
			//Getting the proposed action from the action, given the speeds and the view
			Double action = agent.getMove(view,car);
			
			//Update the car's speed and direction with this action
			car.update(action);
						
			//And update the car through the motion class
			car = motion.move(car);
			
			
     		
			//Update the car in the vector too, of course
			cars.set(i,car);
			
			//And draw it if we want to see it
			if(visible) draw.update(cars);
			
			//calculate accumulative reward
			agent.putReward(car.getCarR());
			if(car.getCarR()!=0){acreward = acreward + 0.3; }
			double reward = 0;
			if (car.getCarR() != 0) reward += 0.3;
			
			
			//In case the checkpoint number changed, we want to know this and reward the agent.
			if(car.getCheckpoint() > checkpoint) {
				
				System.out.print(car.name+":"+checkpoint + "..");
				
				//If he didn't just pass a checkpoint, but finish too, we want to know this and remove it from the running cars.
				if(car.getCheckpoint()==track.getCheckpointsAmount()) {
					
					System.out.print(car.name + ":fin-");
					acreward = 0;
					agent.putReward(reward); //put finish reward
					motion.resetshadow();
					finishedCars.add(car);
					cars.remove(i);
					if(cars.isEmpty()) return false;	//If there aren't any cars left, return a false so we know that we can stop
				}
				else
				{
					agent.putReward(reward);
				}
			}

			//If it didn't pass a checkpoint, it's a BAD AGENT. HERE STUPID AGENT, HAVE A -1. HOW DOES THAT TASTE, NOT VERY NICE I BET
			else {
				agent.putReward(reward-1);				
			}

			car.resetReward();
		}
		
		//Returning true, because not every car is finished yet.
		return true;
		
	}
	
	
	//Simple function that keeps stepping while there are cars.
	public int run() {
		int i = 0;
		boolean running = true;
		while (running) {
			running = step();
			i++;
		}
		//System.out.println("Finished");
		
		//When done, it returns the number of steps it took.
		return i;
	}
	

	
	
}
